//
// PID Controller
//

void initPID() {
  rollPID.SetOutputLimits(MINCOMMAND, MAXCOMMAND);
  pitchPID.SetOutputLimits(MINCOMMAND, MAXCOMMAND);
  yawPID.SetOutputLimits(MINCOMMAND, MAXCOMMAND);
  
  rollPID.SetSampleTime(SAMPLETIME);
  pitchPID.SetSampleTime(SAMPLETIME);
  yawPID.SetSampleTime(SAMPLETIME);
  
  rollPID.SetMode(AUTO);
  pitchPID.SetMode(AUTO);
  yawPID.SetMode(AUTO);
}

void computePID() {
  rollPID.Compute();
  pitchPID.Compute();
  yawPID.Compute();
}
